#!/usr/bin/python

import roslib; roslib.load_manifest('framer')
import rospy
import smach
import smach_ros
from moveHead      import MoveHead
from moveBase      import MoveBase
from pictureTaker  import PictureTaker
from faceProcessor import FaceProcessor
from ruleOfThirds import RuleOfThirds
from rules import RuleChooser


if __name__ == '__main__':
    rospy.init_node('framer')
   
    machine = smach.StateMachine(outcomes=['success', 'fail', 'aborted']) # yes, nothing links to success or fail. Smach doesn't care
    with machine:
        smach.StateMachine.add('Face Processor', FaceProcessor(),
            transitions={'next':'Rule Chooser', 'abort':'aborted'}
        )
        smach.StateMachine.add("Rule Chooser", RuleChooser(),
            transitions={
                'center':'Move Head',
                'rule_of_thirds':'Rule of Thirds',
                #@TODO update with proper transition
                'no_busy_background':'Move Head',
                'abort':'aborted',
            }
        )
        smach.StateMachine.add('Move Head', MoveHead(),
            transitions={
                'success':'Picture Taker',
                'fail':'Face Processor',
                'abort':'aborted'
            }
        )
        smach.StateMachine.add("Rule of Thirds", RuleOfThirds(),
            transitions={
                'success':'Move Base',
                'fail':'Face Processor',
                'abort':'aborted'
            }
        )
        smach.StateMachine.add("Move Base", MoveBase(),
            transitions={
                'success':'Move Head',
                'fail':'Face Processor',
                'abort':'aborted'
            }
        )
        smach.StateMachine.add('Picture Taker', PictureTaker(),
            transitions={
                'success':'Face Processor',
                'abort':'aborted',
                'fail':'Face Processor',
		'busy_background':'Face Processor'
            }
        )

    try:
        sis = smach_ros.IntrospectionServer('framer', machine, '/FRAMER_ROOT')
        sis.start()
        rospy.loginfo("Starting framer state machine.")
        machine.execute()
        rospy.spin()
    except KeyboardInterrupt:
        rospy.loginfo("Shutting down")
